#ifndef _MOVEMENT
#define _MOVEMENT

#include "main.hpp"
#include <string>
using namespace asio;
enum control_mode_t
{
	STABLIZE  =0,
	ACRO      =1,
	ALT_HOLD  =2,
	AUTO      =3,
	GUIDED    =4,
	CIRCLE    =7,
	SURFACE   =9,
	POSHOLD   =16,
	MANUAL    =19,
	MOTOR_DETECT=20,
};

struct Veloc
{
	int x;
	int y;
	int z;
	int r;
};
struct Pose
{
	std::atomic<float> pitchmsg;
	std::atomic<float> rollmsg;
	std::atomic<float> yawmsg;
	std::atomic<float> deepmsg;
};
struct DroneStatus
{
	std::atomic<int> custom_mode;
	std::atomic<int> base_mode;
	std::atomic<int> battery_v;
	std::atomic<int> sys_status;
};
extern UINT8 msgunlock[41];
extern UINT8 msglock[41];

class base_controller
{
public:
	bool Connected=0;
	Veloc vel;
	Pose pose;
	DroneStatus droneStatus;
	float deepBias;
private:
	mavlink_channel_t chan;
	int target; //channel and target of mavlink system
	asio::serial_port& serial; //basic communication method
	std::string mav_errmsg; 
public:
	base_controller(mavlink_channel_t ch,int tar,asio::serial_port& ser):chan(ch),target(tar),serial(ser){
		deepBias = 0;
		pose.yawmsg = 0;
		pose.rollmsg = 0;
		pose.pitchmsg = 0;
		pose.deepmsg = 0;
		droneStatus.battery_v = 0;
	}
	std::string getErrorCode(){
		return this->mav_errmsg;
	}
	void synchronize(UINT8 buffer);
	void doCtrl();
	void pixHeartBeat();
	void setTargetVel(Veloc v);
	void changeMode(int newMode);
	void arm_disarm(bool arm);
	inline bool readyToUse(){
		return this->serial.is_open()&&Connected;
	}

	void async_doCtrl(steady_timer* timer);
	void async_pixHeartBeat(steady_timer* timer);
	void check_stream(steady_timer* timer){
		this->arm_disarm(true);	
		mavlink_msg_request_data_stream_send(chan, target, 190, 0, 10, 1);
		Sleep(100);	
		if(this->droneStatus.battery_v==0&&this->pose.yawmsg==0)
		{
			printmsg("no stream msg,retrying");
			mavlink_msg_request_data_stream_send(chan, target, 190, 0, 10, 1);
			timer->expires_from_now(asio::chrono::seconds(1));
			timer->async_wait(std::bind(&base_controller::check_stream,this,timer));
		}
		else{
			printmsg("connected");
			this->Connected = 1;	
		}
	}
	~base_controller(){}
};

#endif // !_MOVEMENT